#ifndef _ALGO_GYROSCOPE_H
#define _ALGO_GYROSCOPE_H

#include "algorithm_conf.h"
#include "algo_filter.h"

// 测量数据和加速度、角速度的关系
#define ACC_SENSITIVITY         (4095.875f)	// 根据实际配置更改
#define GYRO_SENSITIVITY        (16.388f)

// AHRS姿态解算数据结构体（未使用）
// attitude and heading reference system（航姿参考系统）
//typedef struct _ahrs_elem_t
//{
//    f32 kp;                     // 比例增益
//    f32 ki;                     // 积分增益

//    f32 ex_integral;            // 积分累计误差
//    f32 ey_integral;
//    f32 ez_integral;

//    f32 q0;                     // 四元数
//    f32 q1;
//    f32 q2;
//    f32 q3;

//    f32 rotation_matrix[3][3];  // 旋转矩阵
//} ahrs_elem_t;

// IMU数据结构体
// Inertial measurement unit（惯性测量单元）
typedef struct _algo_imu_t
{
	// 陀螺仪原始数据指针
	struct {
		int16 	*x;
        int16 	*y;
        int16 	*z;
	} acc_raw_ptr, gyro_raw_ptr;
	
	// 加速度计与角速度计
	struct {
		f32		x;
        f32		y;
        f32		z;
	} acc, gyro;
	
	// 角度与角速度
	struct {
		f32 	angle; 	// 角度
		f32		gyro; 	// 角速度
	}pitch, yaw, roll; 	// 俯仰角、偏航角、横滚角

	f32 sampling_period; // 采样周期，单位：秒
} algo_imu_t;

void imu_init(algo_imu_t *imu, f32 sampling_period, int16 *acc_x, int16 *acc_y, int16 *acc_z, int16 *gyro_x, int16 *gyro_y, int16 *gyro_z); // IMU算法初始化
void imu_data_proc(algo_imu_t *imu); 

#endif
